#include "npu_robot/goal_global.h"

void GoalGlobal::init(void)
{
    goalIndex = 0;
    goalNumber = 0;

    goalDirection = GOAL_FORWARD;
}

void GoalGlobal::goalRegister(GoalClass *newGoal)
{
    goalList[goalNumber] = newGoal;
    goalNumber++;
}

GoalClass *GoalGlobal::getNextGoal(bool reset)
{
    if (reset)
    {
        goalIndex = 0;
        goalDirection = GOAL_FORWARD;
    }
    else
    {
        if (goalIndex == 0)
        {
            goalDirection = GOAL_FORWARD;
        }
        else if (goalIndex == goalNumber - 1)
        {
            goalDirection = GOAL_BACKWARD;
        }

        switch (goalDirection)
        {
        case GOAL_FORWARD:
            goalIndex = goalIndex + 1;
            break;

        case GOAL_BACKWARD:
            goalIndex = goalIndex - 1;
            break;
        }
    }

    goal = goalList[goalIndex];

    return goal;
}

GoalGlobal goalGlobal;
